00100		TITLE SERVO
00200		.INSERT ASMBL.FAI[CMS,LCS]
00300	
00400	;Zero page variables.
00500	;Not shared.
00600	
00700	CURVEL:	BLOCK 2	;Commanded velocity.
00800		0
00900	SETPT:	BLOCK 3	;Current setpoint.
01000		0
01100	SETINC:	BLOCK 3	;Interpolating increment for setpoints.
01200	PREDCT:	BLOCK 3	;Result of the predictive term.
01300	LSTINX:	BLOCK 3	;Position at last index pulse.
01400	OLDSP:	BLOCK 3	;Last commanded setpoint, for CMDVEL.
01500	POSERR:	BLOCK 3	;Current position error.
01600	DACSIG:	BLOCK 3	;Scratch.
01700	
01800	;?
01900	CMDBFL ← 4	;# of commands to buffer.
02000	CMDBFO:	BLOCK CMDBFL	;Command FIFO, low byte of data
02100	CMDBF1:	BLOCK CMDBFL	;High byte of data.
02200	CMDBF2:	BLOCK CMDBFL	;Command code.
02300	CMDPUT:	0	;FIFO put pointer.
02400	CMDTAK:	0	;FIFO take pointer.
02500	CMDCTR:	0	;Count of entries in FIFO.
02600	;?
02700	
02800	BGLOCK:	0	;Interlock around background pre. cal.
02900	DSPAT:	BLOCK 2	;Dispatch address when cmds are rcvd?
03000	DSPAT2:	BLOCK 2	;Dispatch when commands are executed?
03100	INCTR:	0	;Count the interpolations.
03200	HSTTMR:	0	;Count ticks between host commands.
03300	
03400	LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
03500	CVSAV:	BLOCK 2	;Save area for background variables.
03600	VELSAV:	BLOCK 2
03700	BGTMP:	BLOCK 2
03800	
03900	ZAPEND ← .-1	;Clear all the above in startup.
04000	
04100	CURPOS:	BLOCK 3	;Current position, extended to 3 bytes.
04200	TL:	0	;Scratch for grey to binary.
04300	TH:	0
04400	
04500	;Shared ram.
04600	   LOC 200	;Second half of zero page.
04700	
04800	STATUS:	0	;Flags for the host.
04900	MODE:	0	;Mode bits from host.
05000	
05100	MEMPTR:	BLOCK 2	;Address pointer for diagnostic read.
05200	NINTER:	0	;# of interpolations between position
05300			;commands.
05400	INTSCL:	0	;# of bits to shift setpoint dif for
05500			;interpolating.
05600	HSTLIM:	0	;# of clock ticks allowed between host
05700		0	;commands.
05800	CMDVEL:	BLOCK 2	;Commanded velocity.
05900	MASS:	BLOCK 2	;Inertia term for prediction.
06000	FRICTN:	BLOCK 2	;Viscous damping coefficient.
06100	GRAVTY:	BLOCK 4	;DC offset for gravity.
06200	POSTOL:	BLOCK 4	;Half-width of position tolerance band.
06300	INTTOL:	BLOCK 4	;Half-width of integration band.
     

00100	START:	CLD
00200		LDXI	STKSIZ	;Setup stack.
00300		TXS
00400	
00500		LDAI	0
00600		LDXI	ZAPEND
00700	RLOOP:	STAZX	0	;Reset ram.
00800		DEX
00900		BPL	RLOOP
01000		STAZ	CURPOS+2
01100	
01200		TAY
01300		BEQ	RSTDEF	;Jump
01400	
01500	
01600	DLOOP:	INY
01700		LDAY	INITBL	;Init ram.
01800		STAZX	0
01900		INY
02000	
02100	RSTDEF:	LDXZY	INITBLBLBL
02200		CPXI	377
02300		BNE	DLOOP
02400	
02500		JSR	POSUPD	;?
02600		JSR	SETCTR	;?
02700	
02800		CLI	;?
02900	
03000		LDAI	4	;?
03100	CKSTAT:	BITZ	IOSTAT	;?
03200		BNE	CKSTAT
03300	
03400	CKNOT:	BITZ	IOSTAT	;?
03500		BEQ	CKNOT
03600	
03700		SEI	;?
03800		LDAI	0	;?
03900		STAZ	IOCTRL	;?
04000	
04100		JMP	START
04200	
04300	INITBL:	STATUS	↔	200
04400		NINTER	↔	=32
04500		INTSCL	↔	5
04600		WHLSIZ	↔	-=24
04700		HSTLIM	↔	=48
04800		DSPAT+1	↔	IMBLK⊗-10
04900		DSPAT2+1   ↔	DFBLK⊗-10
05000		DAC	↔	0
05100		377
     

00100	;Clock tick interrupt.
00200	TIKINT:	PHA	;Save state.
00300		TXA
00400		PHA
00500		TYA
00600		PHA
00700	
00800		LDY	ENCL	;Read encoder.
00900		LDA	ENCH
01000	
01100	;Convert from grey to binary.
01200		STAZ	TH
01300		LSRA
01400		EORZ	TH
01500		STAZ	TH
01600		TAX
01700	
01800		TYA
01900		STAZ	TL
02000		RORA
02100		EORZ	TL
02200		STAZ	TL
02300	
02400		LSRZ	TH
02500		RORA
02600		LSRZ	TH
02700		RORA
02800	
02900		EORZ	TL
03000		STAZ	TL
03100		TAY
03200		TXA
03300		EORZ	TH
03400		STAZ	TH
03500	
03600		LSRA
03700		RORZ	TL
03800		LSRA
03900		RORZ	TL
04000		LSRA
04100		RORZ	TL
04200		LSRA
04300		RORZ	TL
04400	
04500		EORZ	TH
04600		STAZ	TH
04700		TYA
04800		EORZ	TL
04900		EORZ	TH
05000		STAZ	TL	;?
     

00100		JSR	POSUPD	;?
00200	
00300		STAZ	CURPOS
00400		STXZ	CURPOS+1
00500		STYZ	CURPOS+2
00600	
00700		DECZ	HSTTMR
00800		BPL	HOSTOK
00900	
01000		LDAI	0
01100		STAZ	HSTTMR
01200		STAZ	CMDVEL
01300		STAZ	CMDVEL+1
01400	
01500	HOSTOK:	LDAI	4	;?
01600		BITZ	IOCTRL	;?
01700		BNE	INTRS
01800		JMP	CURSRV
01900	
02000	INTRS:	CLC
02100		LDAZ	SETPT-1
02200		ADCZ	SETINC-1
02300		STAZ	SETPT-1
02400		LDAZ	SETPT
02500		ADCZ	SETINC
02600		STAZ	SETPT
02700		LDAZ	SETPT+1
02800		ADCZ	SETINC+1
02900		STAZ	SETPT+1
03000		LDAZ	SETPT+2
03100		ADCZ	SETINC+2
03200		STAZ	SETPT+2
03300	
03400		DECZ	INCTR
03500		BNE	GPOSER
03600	
03700		LDAI	0
03800		STAZ	SETINC-1
03900		STAZ	SETINC
04000		STAZ	SETINC+1
04100		STAZ	SETINC+2
04200	
04300	GPOSER:	SEC
04400		LDAZ	CURPOS
04500		SBCZ	SETPT
04600		STAZ	POSERR
04700		LDAZ	CURPOS+1
04800		SBCZ	SETPT+1
04900		STAZ	POSERR+1
05000		LDAZ	CURPOS+2
05100		SBCZ	SETPT+2
05200		STAZ	POSERR+2
     

00100		BITZ	MODE	;?If servo is disabled, we're
00200		BPL	OOTOL	;automatically out of tolerance
00300	
00400		LDAZ	POSERR+2;Test the sign of pos error.
00500		BMI	NEGPER
00600	
00700		LDAZ	POSTOL	;Positive Compare with tol.
00800		CMPZ	POSERR
00900		LDAZ	POSTOL+1
01000		SBCZ	POSERR+1
01100		LDAI	0
01200		SBCZ	POSERR+2
01300		BCS	TOLOK	;In tolerance.
01400		BCC	OOTOL	;Jump.
01500	
01600	NEGPER:	CLC	;Negative. Add the tolerance.
01700		LDAZ	POSTOL
01800		ADCZ	POSERR
01900		LDAZ	POSTOL+1
02000		ADCZ	POSERR+1
02100		LDAI	0
02200		ADCZ	POSERR+2
02300		BCS	TOLOK	;In tolerance.
02400	
02500	OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
02600		ANDI	177	;Turn off the in tolerance
02700		BNE	WCNTRL	;indicator.
02800	
02900	TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
03000		ORAI	200	;?
03100	WCNTRL:	STAZ	IOCTRL	;?
03200	
03300		BITZ	MODE	;If intergration is disabled,
03400		BVC	OOBAND	;turn it off.
03500		LDAZ	POSERR+2;Test sign of position error.
03600		BMI	ADTOL
03700	
03800		LDAZ	INTTOL	;Positive. Compare with tol.
03900		CMPZ	POSERR
04000		LDAZ	INTTOL+1
04100		SBCZ	POSERR+1
04200		LDAI	0
04300		SBCZ	POSERR+2
04400		BCS	INBAND
04500		BCC	OOBAND
04600	
04700	ADTOL:	CLC	;Negative. Add the tolerance.
04800		LDAZ	INTTOL
04900		ADCZ	POSERR
05000		LDAZ	INTTOL+1
05100		ADCZ	POSERR+1
05200		LDAI	0
05300		ADCZ	POSERR+2
05400		BCS	INBAND
05500	
05600	OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
05700		ORAI	10	;?integration by setting the
05800		ANDI	357	;?control bit. LSB servo off.
05900		BNE	WCTRL2
     

00100	INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled
00200		BITZ	MODE	;?
00300		BEQ	RCNTRL
00400	
00500		LDAZ	POSERR	;Yes. Is the error exactly 0?
00600		ORAZ	POSERR+1
00700		ORAZ	POSERR+2
00800		BNE	RCNTRL
00900	
01000		LDAZ	IOCTRL	;?It is. Integration off, LSB
01100		ORAI	30	;?servo on.
01200		BNE	WCTRL2	;Jump.
01300	
01400	RCNTRL:	LDAZ	IOCTRL	;?LSB disabled or error
01500		ANDI	347	;?not zero. LSB servo off,
01600				;?integration on.
01700	
01800	WCTRL2:	STAZ	IOCTRL	;?
     

00100		LDAZ	LOGTMP	;Since the arithmetic routines
00200		LDYZ	LOGTMP+1;aren't re-entrant, we need to
00300		STAZ	LOGTMP+2;save their state here.
00400		STYZ	LOGTMP+3
00500	
00600		LDYZ	CURVEL	;Get the velocity,
00700		LDAZ	CURVEL+1
00800		JSR	LOG
00900		LDXI	FRICTN	;mult. by the friction
01000		JSR	MUL	;coefficient,
01100		JSR	EXP
01200		TAX
01300		TYA
01400		CLC	;add the position error...
01500		ADCZ	POSERR
01600		STAZ	DACSIG
01700		TXA
01800		ADCZ	POSERR+1
01900		STAZ	DACSIG+1
02000		LDYI	0
02100		TXA	;(sign-extend the velocity)
02200		BPL	NODEY
02300		DEY
02400	
02500	NODEC:	TYA
02600		ADCZ	POSERR+2
02700		STAZ	DACSIG+2
02800	
02900		CLC	;...the velocity predictive term...
03000		LDAZ	DACSIG
03100		ADCZ	PREDCT
03200		STAZ	DACSIG
03300		LDAZ	DACSIG+1
03400		ADCZ	PREDCT+1
03500		STAZ	DACSIG+1
03600		LDAZ	DACSIG+2
03700		ADCZ	PREDCT+2
03800		STAZ	DACSIG+2
03900	
04000		CLC	;...and the gracity offset.
04100		LDAZ	DACSIG
04200		ADCZ	GRAVTY
04300		TAY
04400		LDAZ	DACSIG+1
04500		ADCZ	GRAVTY+1
04600		TAX
04700		LDAZ	DACSIG+2
04800		ADCZ	GRAVTY+2
04900	
05000		JSR	PUTDAC	;Put result out to the DAC.
05100	
05200		LDYZ	LOGTMP+3;Restore the arithmetic
05300		LDAZ	LOGTMP+2;routines' state.
05400		STYZ	LOGTMP+1
05500		STAZ	LOGTMP
05600	
05700	CMDSP:
     

00100	CMDEND:	LDAI	4	;Done with commands.
00200		BITZ	IOCTRL	;Are we servoing?
00300		BEQ	INTXIT
00400		BITZ	BGLOCK	;Yes. Is the background
00500		BMI	INTXIT	;predictor still running?
00600	
00700		DECZ	BGLOCK	;No. Start it up.
00800		JMP	BGSRV
00900	
01000	BGDON:	INCZ	BGLOCK	;Unlock?
01100	
01200	INTXIT:	PLA	;Restore state and dismiss interrupt.
01300		TAY
01400		PLA
01500		TAX
01600		PLA
01700		RTI
01800	
01900	;Background velocity prediction.
02000	BGSRV:	LDAZ	CURVEL	;Copy the variables used to
02100		STAZ	VELSAV	;avoid interference from
02200		LDAZ	CURVEL+1;interrupts while this routine
02300		STAZ	VELSAV+1;is running.
02400		LDAZ	CMDVEL
02500		STAZ	CVSAV
02600		LDAZ	CMDVEL+1
02700		STAZ	CVSAV+1
02800		LDYZ	POSERR
02900		LDAZ	POSERR+1
03000		LDXZ	POSERR+2
03100	
03200		CLI	;Enable interrupts?
03300	
03400		PHA
03500		ASLA	;Is magnitude of position error
03600		TXA	;< 2↑15?
03700		ADCI	0
03800		BEQ	FLOERR
03900	
04000		PLA	;No. Set the predictive term to zero.
04100		LDAI	0
04200		TAX
04300		TAY
04400		JMP	NTRLOC
04500	
04600	FLOERR:	PLA	;Yes. Float the position error.
04700		JSR	LOG
04800		JSR	INV	;TMP = 1 / POSERR
04900		STYZ	BGTMP
05000		STAZ	BGTMP+1
05100		CLC
05200		LDAZ	CVSAV	;Commanded velocity + current
05300		ADCZ	VELSAV	;velocity...
05400		TAY
05500		LDAZ	CVSAV+1
05600		ADCZ	VELSAV+1
     

00100		JSR	LOG	;...float...
00200		LDXI	BGTMP
00300		JSR	MUL	;...* TMP...
00400		STYZ	BGTMP	;...stored at TMP.
00500		STAZ	BGTMP+1
00600		SEC
00700		LDAZ	CVSAV	;Commanded velocity - current
00800		SBCZ	VELSAV	;velocity...
00900		TAY
01000		LDAZ	CVSAV+1
01100		SBCZ	VELSAV+1
01200		JSR	LOG	;...same thing.
01300		LDXI	BGTMP
01400		JSR	MUL
01500		STYZ	BGTMP
01600		STAZ	BGTMP+1
01700	
01800		SEI	;?Interlock...
01900	
02000		LDYZ	;...get the mass...
02100		LDAZ	MASS+1
02200	
02300		CLI	;?clear the lock.
02400	
02500		JSR	MUL	;Scale the predictor.
02600		JSR	EXP	;Back to integer form.
02700		LDXI	0
02800		CMPI	0
02900		BPL	NTRLOC	;Extend sign to 3 bytes.
03000		DEX
03100	
03200	NTRLOC:	SEI	;End of background. Interlock.
03300	
03400		STYZ	PREDCT
03500		STAZ	PREDCT+1;Store the result for the servo
03600		STXZ	PREDCT+2;to use.
03700		JMP	BGDON
     

00100	;Subroutines?
00200	;Enter with position in A (low), X (middle), Y (high).
00300	;Sets current position to that value, puts the setpoint
00400	;to the same, clears the setpoint interpolating
00500	;increment, and goes into stop mode.
00600	SETCTR:	PHA
00700		SEC	;Get low byte of position change,
00800		SBCZ	CURPOS
00900		CLC
01000		ADCZ	POSOFF	;add it to the counter offset?
01100		STAZ	POSOFF
01200		PLA
01300		STAZ	CURPOS	;Set the current position.
01400		STXZ	CURPOS+1
01500		STYZ	CURPOS+2
01600	
01700	;Second entry - freeze to the position in A, X, Y as
01800	;above without changing the current position.
01900	FREEZE:	STAZ	SETPT	;Set the position command.
02000		STXZ	SETPT+1
02100		STYZ	SETPT+2
02200		STAZ	OLDSP
02300		STXZ	OLDSP+1
02400		STYZ	OLDSP+2
02500	
02600		LDAI	75	;I/O control bits for servo
02700		STAZ	IOCTRL	;?enable on, all others off.
02800	
02900		LDAI	0
03000		STAZ	SETPT-1	;Clear the setpoint extension
03100		STAZ	SETINC-1;and the interpolator
03200		STAZ	SETINC
03300		STAZ	SETINC+1
03400		STAZ	SETINC+2
03500		STAZ	CMDVEL	;and the commanded velocity.
03600		STAZ	CMDVEL+1
03700	
03800		LDAZ	SETPT	;Return the regs. unchanged.
03900		RTS
04000	;Enter with low counter value in Y.
04100	;Returns updated position in A (low), X (middle),
04200	;Y (high). Also sets CURVEL to the 16-bit signed
04300	;velocity.
04400	POSUPD:	TYA	;?Add the counter offset to get
04500		CLC	;?the low byte of the position.
04600		ADCZ	POSOFF	;?
04700		STAZ	DACSIG	;Save that value.
04800		LDXI	0
04900		SEC
05000		SBCZ	CURPOS	;Subtract the old position
05100		STAZ	CURVEL	;yielding the velocity.
05200		BPL	SVVEL
05300		DEX	;Extend sign to 16 bits.
05400	SVVEL:	STXZ	CURVEL+1
05500		LDXZ	CURPOS+1	;Set up for updating bytes
05600		LDYZ	CURPOS+2	;2 and 3.
05700		LDAZ	DACSIG	;Did bit 7 of position change?
05800		EORZ	CURPOS
05900		BPL	GETDAC	;If not, we're through.
06000		LDAZ	CURVEL	;It did. Which way did we move
06100		BMI	DOWN
06200		LDAZ	DACSIG	;Upward.
06300		BMI	GETDAC	;If bit 7 is on, we're done.
06400		INX	;Off. Increment middle byte
06500		BNE	GETDAC
06600		INY	;and high byte if necessary.
06700		JMP	GETDAC
06800	
06900	DOWN:	LDAZ	DACSIG	;Downward.
07000		BPL	GETDAC	;If bit 7 is off, we're done.
07100		CPXI	0	;On.
07200		BNE	DX
07300		INY	;Increment high byte if necessary
07400	DX:	DEX	;and middle byte.
07500	
07600	GETDAC:	LDAZ	DACSIG
07700		RTS
     

00100	;DAC output subroutine. Not sub?
00200	;Enter with 3 byte value in Y (low), X (middle),
00300	;A (high). Clobbers all registers, but the 8 bits the
00400	;DAC got are returned in?
00500	PUTDAC:	BMI	NEGDAG	;Assuming the last I. loaded A.
00600		CPYI	200	;Positive. Compare with 2↑7.
00700		BCS	TOOHI
00800		CPXI	1
00900		SBCI	0
01000		BCC	INRNGE
01100	
01200	TOOHI:	LDYI	177	;Too high. Saturate positive.
01300		BNE	INRNGE	;Jump.
01400	
01500	NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
01600		BCC	TOOLOW
01700	
01800		CPXI	377
01900		SBCI	377
02000		BCS	INRNGE
02100	
02200	TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.
02300	
02400	INRNGE:	STY	DAC	;Output 8 bits to the DAC.
02500		RTS
02600	
02700	DOUBLE:	PHA	;Doubles the position in (Y,X,A) if
02800		LDAI	DBLMOD	;the double mode bit is set.
02900		BITZ	MODE
03000		BEQ	NOTDBL
03100		PLA
03200		ASLA
03300		PHA
03400		TXA
03500		ROLA
03600		TAX
03700		TYA
03800		ROLA
03900		TAY
04000	NOTDBL:	PLA
04100		RTS
04200	
04300	HALVE:	PHA	;Halve the position argument in (Y,X,A)
04400		LDAI	DBLMOD	;if the double mode bit is set.
04500		BITZ	MODE
04600		BEQ	NOTDBL
04700		TYA
04800		CMPI	200
04900		RORA
05000		TAY
05100		TXA
05200		RORA
05300		TAX
05400		PLA
05500		RORA
05600		RTS
     

00100	ENBTST:	PHA	;Test for servo enabled and not locked
00200		LDAZ	MODE	;on the wheel index.
00300		ANDI	202
00400		CMPI	200
00500		BNE	NOTENB
00600		PLA	;OK. Return.
00700	
00800	NOTENB:	PLA	;No. Wipe the return address and
00900		PLA	;end this command.
01000		PLA
01100		JMP	CMDEND
     

00100	;Enter with high byte in A, low in Y.
00200	;Returns A = characteristic and sign, Y = mantissa.
00300	;Clobbers X, LOGTMP, LOGTMP+1.
00400	LOG:	STYZ	LOGTMP	;Save the inputs.
00500		STAZ	LOGTMP+1
00600	
00700		LDXI	20+100	;?Init characteristic to 15.
00800		CMPI	0	;Test sign of input.
00900		BPL	POSIN
01000		SEC	;Negative. 2's complement it.
01100		LDAI	0
01200		SBCZ	LOGTMP
01300		STAZ	LOGTMP
01400		LDAI	0
01500		SBCZ	LOGTMP+1
01600	POSIN:	BNE	NORML	;Is high byte zero?
01700		LDAZ	LOGTMP	;Yes. Low byte?
01800		BEQ	RTRN	;If so, return zero.
01900		LDYI	0	;Low nonzero. Shift left one
02000		STYZ	LOGTMP	;byte,
02100		LDXI	10+100	;?change characteristic to 7.
02200	NORML:	DEX	;Normalize the number, counting the
02300		ASLZ	LOGTMP	;characteristic down. When the
02400		ROLA	;first "1" shifts out, we've subtracted
02500		BCC	NORML	;1 from the normalized number
02600		ASLZ	LOGTMP	;(This rounds the result)
02700		ADCI	=11	;and are left with the fraction
02800		TAY	;Adding 11 to that is equivalent to
02900		TXA	;adding 0.043.
03000		ADCI	0	;Propagate the carry into the
03100				;characteristic.
03200		ASLA	;Insert the sign bit from the saved
03300		ASLZ	LOGTMP+1;input.
03400		RORA
03500	RTRN:	RTS	;Done.
03600	
03700	;Enter with sign and characteristic in A, mantissa in Y
03800	;Returns 16-bit integer, low byte in Y, high in A.
03900	;Clobbers X, LOGTMP, LOGTMP+1.
04000	EXP:	STAZ	LOGTMP+1;Save sign of input.
04100		ANDI	177	;Mask it off.
04200		BEQ	ZEROIN	;Zero characteristic returns
04300		TAX	;zero.
04400		TYA	;Get the mantissa...
04500		SEC
04600		SBCI	=11	;...subtract 0.043...
04700		STAZ	LOGTMP	;(save this value)
04800		TXA	;...propagate the carry and get rid
04900		SBCI	100	;of the XS-64 offset.
05000		BMI	NEGIN	;If negative (value < 1.0)
05100				;return zero.
05200		CMPI	=15	;Test for overflow (value>=2↑15
05300		BCS	SATUR
05400		TAX	;...no. Number is in range.
05500		ADCI	-10	;?Is characteristic below 8?
05600		BMI	BLOATE
05700		TAX	;No. Reduce if by 8,
05800		JSR	UNNORM	;unnormalize.
05900		BMI	GETTMP	;Jump.
     

00100	BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
00200		ASLZ	LOGTMP	;(round result)
00300		ADCI	0
00400		STAZ	LOGTMP	;use result as low byte and
00500		LDAI	0	;set high byte to zero.
00600	
00700	GETTMP:	LDYZ	LOGTMP
00800	GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
00900		BPL	POSIGN
01000		STAZ	LOGTMP+1;...negative. 2's complement
01100		LDAI	0	;the result.
01200		SEC
01300		SBCZ	LOGTMP
01400		TAY
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIGN:	RTS
01800	
01900	NEGIN:	LDAI	0	;Set the result to zero if the
02000	ZEROIN:	TAY	;input is negative.
02100		RTS
02200	
02300	SATUR:	LDYI	OFF	;Saturate result to 2↑15 - 1 if
02400		STYZ	LOGTMP	;input was 15 or more.
02500		LDAI	177
02600		BNE	GTMP1	;Jump.
02700	
02800	UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
02900		BNE	DECRX	;to the fraction.
03000	
03100	SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
03200		ROLA	;amount of the characteristic.
03300	DECRX:	DEX
03400		BPL	SCALE
03500		RTS
03600	
03700	;Enter with characteristic of multiplier in A,
03800	;mantissa in Y, X pointing to a pair of base page
03900	;locations containing the multiplicand (mantissa in the
04000	;low byte).
04100	;Returns the product in A and Y, same form as the
04200	;multiplier. Leaves X unchanged. Clobbers LOGTMP and
04300	;LOGTMP+1.
04400	MUL:	PHA
04500		EORZX	1	;Compute sign of result,
04600		STAZ	LOGTMP+1	;save it away.
04700		PLA
04800		ANDI	177	;Mask off multiplier sign.
04900		BEQ	ZEROIN	;If zero, return zero.
05000		STAZ	LOGTMP
05100		TYA	;Add the two logarithms.
05200		CLC
05300		ADCZX	0
05400		TAY
05500		LDAZX	1
05600		ANDI	177	;If multiplicand is zero,
05700		BEQ	ZEROIN	;return a zero.
05800		ADCZ	LOGTMP
05900		SEC
06000		SBCI	100	;Correct the XS-64 offset.
     

00100		BPL	INSIGN	;Result in range?
00200		ANDI	100	;No. If underflow,
00300		BNE	NEGIN	;return zero.
00400		LDAI	177	;Overflow. Saturate to
00500		LDYI	377	;highest magnitude.
00600	
00700	INSIGN:	ASLA	;Insert the sign of the result.
00800		ASLZ	LOGTMP+1
00900		RORA
01000		RTS
01100	
01200	;Inverse function: 2's complement the magnitude part
01300	;of a 15-bit logarithm.
01400	;Enter with characteristic in A, mantissa in Y.
01500	;Returns inverse in the same form. X unchanged.
01600	;Clobbers LOGTMP and LOGTMP+1.
01700	INV:	STYZ	LOGTMP	;Pretty straightforward...
01800		STAZ	LOGTMP+1
01900		SEC
02000		LDAI	0	;Complement the number by
02100		SBCZ	LOGTMP	;subtracting it from zero.
02200		TAY
02300		LDAI	0
02400		SBCZ	LOGTMP+1
02500		JMP	INSIGN	;Insert the original sign.
02600	END